
/**********************************************************************************\
** 文件名称: motor.c
** 创建人员: YCG
** 创建日期: 2018-12-15
** 文档描述:
** 版本代号: V0.1
** 版本说明: 初始版本
\********************************************************************************/

#include "motor.h"
#include "uart.h"

uint32_t HighPriTime = 0;    //高优先级速度时间
uint32_t LowPriTime = 0;     //低优先级速度时间
uint32_t MotorSysTime = 0;   //电机控制的系统时间

float LastRightSpeed = 0;     //上一次右轮的速度
float LastLeftSpeed = 0;      //上一次左轮的速度

static uint8_t Flag = 0;     //用来控制避免重复操作SpeedCtrl函数，很费时间的，只要执行一次就行了
uint8_t ChangeFlag = 0;

/*******************************************************************************
** 函数名称: MoterTimeScan
** 功能描述: 电机控制扫描函数
** 参数说明: time: [输入] 系统时间
** 返回说明: None
** 创建人员: YCG
** 创建日期: 2018-12-15
********************************************************************************/
void MoterTimeScan(uint32_t time) {
    MotorSysTime = time;                       //更新系统时间
    if(time < HighPriTime) {                   //高优先级的速度控制，用来控制转向之类的
        if(Flag != 0) {
            Flag = 0;
           // Uprintf("Flag:%d\r\n", Flag);
        }
    } else if(time < LowPriTime) {             //高优先执行完后会执行低优先级的速度控制，正常行驶状态
        if(Flag != 1) {
            Flag = 1;
            SetSpeed(LastLeftSpeed, LastRightSpeed); //将速度切换回来
          //  Uprintf("Flag:%d\r\n", Flag);
        }
    } else {
        if(Flag != 2) {
            Flag = 2;
            SetSpeed(0, 0);   //停止运行
           // Uprintf("Flag:%d\r\n", Flag);
        }                               //运行时间执行完毕后停止
    }
}

/*******************************************************************************
** 函数名称: SpeedCtrl
** 功能描述: 车速控制代码
** 参数说明:
            left: 左轮速度，正：向前，负：向后，零：刹车
            right:左轮速度，正：向前，负：向后，零：刹车
** 返回说明: None
** 创建人员: YCG
** 创建日期: 2018-11-15
********************************************************************************/
void SpeedCtrl(float left, float right, uint16_t t, uint8_t mode)  // 前进
{
    //高优先级速度设置处理
    if(mode == MOTOR_HIGH_PRI) {
        HighPriTime = MotorSysTime + t;    //添加高优先级的速度运行时间
        if(LowPriTime >= MotorSysTime) {   //低优先级的速度还在运行
            LowPriTime += t;               //低优先级速度往后延时这么久
        }
        SetSpeed(left, right);             //立刻更新速度
    }
		
    //低优先级速度设置处理
    if(mode == MOTOR_LOW_PRI) {
        //设置的速度与上次不相同，或者 已经没有运行时间了
        if((LastRightSpeed != right) || (LastLeftSpeed != left) || MotorSysTime >= LowPriTime) {
            LowPriTime = MotorSysTime + t;  //根据系统时间延时相应的设置时间
            MOTOR_Printf("LowPriTime:%d\r\n", LowPriTime);
        } else {
            LowPriTime += t;      //继承上一次没有运行完的时间
            if(LowPriTime - MotorSysTime > MOTOR_MAX_TIME) { //如果大于最大可以继承的时间
                LowPriTime += MOTOR_MAX_TIME; //约束一下时间
                MOTOR_Printf("LowPriTime~~:%d\r\n", LowPriTime);
            }
        }
        Flag = 1;
        LastRightSpeed = right;
        LastLeftSpeed = left	;
        if(MotorSysTime > HighPriTime) {   //只有没有高优先级运行的时候才更新
            SetSpeed(left, right);
        }
    }
}

#ifndef  MOTOR_USE_SERVO              //使用定时器的输出比较模式进行控制
/*******************************************************************************
** 函数名称: MoterInit
** 功能描述: 电机相关的定时器初始化
** 参数说明: count: [输入] 定时器的计数值
** 返回说明: None
** 创建人员: YCG
** 创建日期: 2018-12-15
********************************************************************************/
void MoterInit(uint16_t count) {

    LL_TIM_EnableIT_CC1(MOTOR_TIME);       //使能输出比较中断
    LL_TIM_EnableIT_CC2(MOTOR_TIME);       //使能输出比较中断
    LL_TIM_EnableIT_CC3(MOTOR_TIME);       //使能输出比较中断
    LL_TIM_EnableIT_CC4(MOTOR_TIME);       //使能输出比较中断

    LL_TIM_EnableIT_UPDATE(MOTOR_TIME);    //使能计数器更新中断

    LL_TIM_SetAutoReload(MOTOR_TIME, count);  //设置定时器的更新值

    LL_TIM_EnableCounter(MOTOR_TIME);         //打开计数器开始计数
}
/*******************************************************************************
** 函数名称: SetSpeed
** 功能描述: 车速控制代码
** 参数说明:
            left: 左轮速度，正：向前，负：向后，零：刹车
            right:左轮速度，正：向前，负：向后，零：刹车
** 返回说明: None
** 创建人员: YCG
** 创建日期: 2018-11-15
********************************************************************************/
void SetSpeed(float left, float right) {
    left = left * 1;
    right = (float)right * 1;
    if (left > 0) {                   //向前进
        L_INA_PwmSet(0);
        L_INB_PwmSet(0 - left);
    } else if (left < 0) {            //向后退
        L_INA_PwmSet(left);
        L_INB_PwmSet(0);
    } else if (left == 0) {           //刹车
        L_INA_PwmSet(0);
        L_INB_PwmSet(0);
    }
    if (right > 0) {                 //向前进
        R_INA_PwmSet(0);
        R_INB_PwmSet(0 - right);
    }
    else if (right < 0) {            //向后退
        R_INA_PwmSet(right);
        R_INB_PwmSet(0);
    }
    else if (right == 0) {           //刹车
        R_INA_PwmSet(0);
        R_INB_PwmSet(0);
    }
}
/*******************************************************************************
** 函数名称: Motor_IRQHandler
** 功能描述: 电机中断服务函数，放入相应的定时器中断中
** 参数说明: None
** 返回说明: None
** 创建人员: YCG
** 创建日期: 2018-12-15
********************************************************************************/
void Motor_IRQHandler(void) {
    if(LL_TIM_IsActiveFlag_UPDATE(MOTOR_TIME)) {      //更新中断
        LL_TIM_ClearFlag_UPDATE(MOTOR_TIME);          //清除中断标志位

        if( LL_TIM_OC_GetCompareCH1(MOTOR_TIME) > LL_TIM_GetCounter(MOTOR_TIME) )  //防止占空比为零时，还有脉冲
            LL_GPIO_SetOutputPin( L_INA_GPIO_Port, L_INA_Pin);
        if( LL_TIM_OC_GetCompareCH2(MOTOR_TIME) > LL_TIM_GetCounter(MOTOR_TIME) )
            LL_GPIO_SetOutputPin( L_INB_GPIO_Port, L_INB_Pin);

        if( LL_TIM_OC_GetCompareCH3(MOTOR_TIME) > LL_TIM_GetCounter(MOTOR_TIME) )
            LL_GPIO_SetOutputPin(R_INA_GPIO_Port, R_INA_Pin);

        if( LL_TIM_OC_GetCompareCH4(MOTOR_TIME) > LL_TIM_GetCounter(MOTOR_TIME) )
            LL_GPIO_SetOutputPin( R_INB_GPIO_Port, R_INB_Pin);
    }
    if(LL_TIM_IsActiveFlag_CC1(MOTOR_TIME)) {        //比较中断

        LL_TIM_ClearFlag_CC1(MOTOR_TIME);            //清除中断标志位
        if( LL_TIM_OC_GetCompareCH1(MOTOR_TIME) <= LL_TIM_GetCounter(MOTOR_TIME) )   //防止占空比过高时出现翻转现象
            LL_GPIO_ResetOutputPin( L_INA_GPIO_Port, L_INA_Pin);
    }
    if(LL_TIM_IsActiveFlag_CC2(MOTOR_TIME)) {        //比较中断
        LL_TIM_ClearFlag_CC2(MOTOR_TIME);            //清除中断标志位
        if( LL_TIM_OC_GetCompareCH2(MOTOR_TIME) <= LL_TIM_GetCounter(MOTOR_TIME) )
            LL_GPIO_ResetOutputPin( L_INB_GPIO_Port, L_INB_Pin);

    }
    if(LL_TIM_IsActiveFlag_CC3(MOTOR_TIME)) {        //比较中断
        LL_TIM_ClearFlag_CC3(MOTOR_TIME);            //清除中断标志位
        if( LL_TIM_OC_GetCompareCH3(MOTOR_TIME) <= LL_TIM_GetCounter(MOTOR_TIME) )
            LL_GPIO_ResetOutputPin(R_INA_GPIO_Port, R_INA_Pin);

    }
    if(LL_TIM_IsActiveFlag_CC4(MOTOR_TIME)) {        //比较中断
        LL_TIM_ClearFlag_CC4(MOTOR_TIME);            //清除中断标志位
        if( LL_TIM_OC_GetCompareCH4(MOTOR_TIME) <= LL_TIM_GetCounter(MOTOR_TIME) )
            LL_GPIO_ResetOutputPin( R_INB_GPIO_Port, R_INB_Pin);
    }
}

#else   //这里使用舵机库进行PWM控制
SerHead_t MotorHead[1];   //定义电机控制头
SerItem_t L_INA[1];       //定义电机通道控制对象
SerItem_t L_INB[1];       //定义电机通道控制对象
SerItem_t R_INA[1];       //定义电机通道控制对象
SerItem_t R_INB[1];       //定义电机通道控制对象
/*******************************************************************************
** 函数名称: MoterInit
** 功能描述: 电机初始化
** 参数说明: None
** 返回说明: None
** 创建人员: YCG
** 创建日期: 2018-12-15
********************************************************************************/
void MoterInit(uint16_t count) {
    ServoHeadInit(MotorHead, count, 100, 0, 20, 20);
    ServoPinInit(MotorHead, L_INA, L_INA_GPIO_Port, L_INA_Pin);
    ServoPinInit(MotorHead, L_INB, L_INB_GPIO_Port, L_INB_Pin);
    ServoPinInit(MotorHead, R_INA, R_INA_GPIO_Port, R_INA_Pin);
    ServoPinInit(MotorHead, R_INB, R_INB_GPIO_Port, R_INB_Pin);
    ListPrint(&MotorHead->List);

    LL_TIM_OC_DisablePreload(MOTOR_TIME, LL_TIM_CHANNEL_CH1);  //不使能通道的预装载，这样每次装载比较值立即生效
    LL_TIM_EnableIT_CC1(MOTOR_TIME);       //使能输出比较中断
    LL_TIM_EnableIT_UPDATE(MOTOR_TIME);    //使能计数器更新中断
    LL_TIM_SetAutoReload(MOTOR_TIME, count);  //设置定时器的更新值
    LL_TIM_EnableCounter(MOTOR_TIME);      //打开计数器开始计数
}

/*******************************************************************************
** 函数名称: SetSpeed
** 功能描述: 车速控制代码
** 参数说明:
            left: 左轮速度，正：向前，负：向后，零：刹车
            right:左轮速度，正：向前，负：向后，零：刹车
** 返回说明: None
** 创建人员: YCG
** 创建日期: 2018-11-15
********************************************************************************/
void SetSpeed(float left, float right)  // 前进
{
    left = left * 1;
    right = (float)right * 1;
    if (left > 0) {                 //向前进
        ServoSet(L_INA, left);
        ServoSet(L_INB, 0);
    } else if (left < 0) {           //向后退
        ServoSet(L_INA, 0);
        ServoSet(L_INB, 0 - left);
    } else if (left == 0) {           //刹车
        ServoSet(L_INA, 0);
        ServoSet(L_INB, 0);
    }
    if (right > 0) {                 //向前进
        ServoSet(R_INA, right);
        ServoSet(R_INB, 0);
    }
    else if (right < 0) {           //向后退
        ServoSet(R_INA, 0);
        ServoSet(R_INB, 0 - right);
    }
    else if (right == 0) {           //刹车
        ServoSet(R_INA, 0);
        ServoSet(R_INB, 0);
    }
    ListPrint(&MotorHead->List);
}

/*******************************************************************************
** 函数名称: Motor_IRQHandler
** 功能描述: 电机中断服务函数
** 参数说明: None
** 返回说明: None
** 创建人员: YCG
** 创建日期: 2018-12-15
********************************************************************************/
void Motor_IRQHandler(void) {
    uint32_t nextTime = 0; //下一次比较的时间
    if(LL_TIM_IsActiveFlag_UPDATE(MOTOR_TIME)) {      //更新中断
        LL_TIM_ClearFlag_UPDATE(MOTOR_TIME);          //清除中断标志位
        LL_TIM_ClearFlag_CC1(MOTOR_TIME);            //清除中断标志位
        nextTime = ServoUpdate(MotorHead, 0);     //输入零表示更新
    } else if(LL_TIM_IsActiveFlag_CC1(MOTOR_TIME)) {        //比较中断
        LL_TIM_ClearFlag_CC1(MOTOR_TIME);            //清除中断标志位
        nextTime = ServoUpdate(MotorHead, LL_TIM_OC_GetCompareCH1(MOTOR_TIME)); //用当前的比较值作为时间输入
    }
    if(nextTime != 0) {  //等于零的时候是已经到末尾了
        LL_TIM_OC_SetCompareCH1(MOTOR_TIME, nextTime);  //设置下一次比较的值
    }
}
#endif
/********************************End of File************************************/


